#ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_CORRELATIVE_SCAN_MATCHER_2D_H_
#define CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_CORRELATIVE_SCAN_MATCHER_2D_H_

#include <vector>

#include "Eigen/Core"
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/mapping/2d/grid_2d.h"
#include "cartographer/mapping/2d/map_limits.h"
#include "cartographer/mapping/2d/xy_index.h"
#include "cartographer/sensor/point_cloud.h"

namespace cartographer
{
    namespace mapping
    {
        namespace scan_matching
        {
            typedef std::vector<Eigen::Array2i> DiscreteScan2D;

            // Describes the search space.
            struct SearchParameters
            {
                // Linear search window in pixel offsets; bounds are inclusive.
                struct LinearBounds
                {
                    int min_x;
                    int max_x;
                    int min_y;
                    int max_y;
                };

                SearchParameters(double linear_search_window, double angular_search_window, const sensor::PointCloud &point_cloud,
                                 double resolution);

                // For testing.
                SearchParameters(int num_linear_perturbations, int num_angular_perturbations, double angular_perturbation_step_size,
                                 double resolution);

                // Tightens the search window as much as possible.
                void ShrinkToFit(const std::vector<DiscreteScan2D> &scans, const CellLimits &cell_limits);

                int num_angular_perturbations;
                double angular_perturbation_step_size;
                double resolution;
                int num_scans;
                std::vector<LinearBounds> linear_bounds; // Per rotated scans.
            };

            // Generates a collection of rotated scans.
            std::vector<sensor::PointCloud> GenerateRotatedScans(const sensor::PointCloud &point_cloud,
                                                                 const SearchParameters &search_parameters);

            // Translates and discretizes the rotated scans into a vector of integer
            // indices.
            std::vector<DiscreteScan2D> DiscretizeScans(const MapLimits &map_limits, const std::vector<sensor::PointCloud> &scans,
                                                        const Eigen::Translation2f &initial_translation);

            // A possible solution.
            struct Candidate2D
            {
                Candidate2D()
                    : id(0)
                    , scan_index(0)
                    , x_index_offset(0)
                    , y_index_offset(0)
                    , x(0)
                    , y(0)
                    , orientation(0)
                    , score(0.0)
                {
                    weight_exp = 0;
                    weight_score = 0;
                    weight_ratio = 0;
                }

                Candidate2D(int pid, int i, int x_offset, int y_offset, double yaw, double resolution)
                    : id(pid)
                    , scan_index(i)
                    , x_index_offset(x_offset)
                    , y_index_offset(y_offset)
                    , x(-y_offset * resolution)
                    , y(-x_offset * resolution)
                    , orientation(yaw)
                    , score(0.0)
                {
                    weight_exp = 0;
                    weight_score = 0;
                    weight_ratio = 0;
                }

                Candidate2D(int pid, int i, double x, double y, double yaw)
                    : id(pid)
                    , scan_index(i)
                    , x_index_offset(-1)
                    , y_index_offset(-1)
                    , x(-y)
                    , y(-x)
                    , orientation(yaw)
                    , score(0.0)
                {
                    weight_exp = 0;
                    weight_score = 0;
                    weight_ratio = 0;
                }

                Candidate2D(const Candidate2D &d)
                    : id(d.id)
                    , scan_index(d.scan_index)
                    , x_index_offset(d.x_index_offset)
                    , y_index_offset(d.y_index_offset)
                    , x(d.x)
                    , y(d.y)
                    , orientation(d.orientation)
                    , score(d.score)
                {
                    weight_exp = 0;
                    weight_score = 0;
                    weight_ratio = 0;
                }

                Candidate2D(const int init_scan_index, const int init_x_index_offset, const int init_y_index_offset,
                            const SearchParameters &search_parameters)
                    : id(0)
                    , scan_index(init_scan_index)
                    , x_index_offset(init_x_index_offset)
                    , y_index_offset(init_y_index_offset)
                    , x(-y_index_offset * search_parameters.resolution)
                    , y(-x_index_offset * search_parameters.resolution)
                    , orientation((scan_index - search_parameters.num_angular_perturbations) *
                                  search_parameters.angular_perturbation_step_size)
                {
                    weight_exp = 0;
                    weight_score = 0;
                    weight_ratio = 0;
                }

                Candidate2D(const int init_scan_index, const int init_x_index_offset, const int init_y_index_offset,
                            const SearchParameters &search_parameters, const int curr_resolution_depth)
                    : id(0)
                    , scan_index(init_scan_index)
                    , x_index_offset(init_x_index_offset)
                    , y_index_offset(init_y_index_offset)
                    , x(-y_index_offset * search_parameters.resolution)
                    , y(-x_index_offset * search_parameters.resolution)
                    , orientation((scan_index - search_parameters.num_angular_perturbations) *
                                  search_parameters.angular_perturbation_step_size)
                    , resolution_depth(curr_resolution_depth)
                {
                    weight_exp = 0;
                    weight_score = 0;
                    weight_ratio = 0;
                }

                int id = 0;
                // Index into the rotated scans vector.
                int scan_index = 0;
                // Linear offset from the initial pose.
                int x_index_offset = 0;
                int y_index_offset = 0;
                // Pose of this Candidate2D relative to the initial pose.
                double x = 0.;
                double y = 0.;
                double orientation = 0.;
                int resolution_depth = -1;
                // Score, higher is better.
                float score = 0.f;
                double weight_exp;
                double weight_score;
                double weight_ratio;

                bool operator<(const Candidate2D &other) const
                {
                    return score < other.score;
                }
                bool operator>(const Candidate2D &other) const
                {
                    return score > other.score;
                }
            };

        } // namespace scan_matching
    }     // namespace mapping

    class RealTimeScanMatcher2D
    {
    public:
        RealTimeScanMatcher2D()
        {
        }

        virtual ~RealTimeScanMatcher2D()
        {
        }

        virtual double Match(const transform::Rigid2d &initial_pose_estimate, const sensor::PointCloud &point_cloud,
                             const mapping::Grid2D &grid, transform::Rigid2d *pose_estimate) = 0;

        virtual void Failed()
        {
        }

        virtual int Success(const transform::Rigid2d &pose_init, const transform::Rigid2d &pose_opt)
        {
            return 0;
        }
    };
} // namespace cartographer

#endif // CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_CORRELATIVE_SCAN_MATCHER_2D_H_
